#include <cstdio>
#include "cmvutils.h"

namespace cmv {

void rgb2uyvy(unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
	register int i = int(NumPixels + (NumPixels << 1) - 1); // 3 * NumPixels - 1
	register int j = int((NumPixels << 1) - 1); // 2 * NumPixels - 1
	register int y0, y1, u0, u1, v0, v1 ;
	register int r, g, b;

	while (i > 0) {
		// opencv中存储顺序为bgr
		r = (unsigned char)src[i--];
		g = (unsigned char)src[i--];
		b = (unsigned char)src[i--];
		rgb2yuv(r, g, b, y0, u0 , v0);
		r = (unsigned char)src[i--];
		g = (unsigned char)src[i--];
		b = (unsigned char)src[i--];
		rgb2yuv(r, g, b, y1, u1 , v1);
		dest[j--] = y0;
		dest[j--] = (v0+v1) >> 1;
		dest[j--] = y1;
		dest[j--] = (u0+u1) >> 1;
	}
}

void uyvy2rgb(unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
	register int i = int((NumPixels << 1) - 1); // 2 * NumPixels - 1
	register int j = int(NumPixels + (NumPixels << 1) - 1);  // 3 * NumPixels - 1
	register int y0, y1, u, v;
	register int r, g, b;

	while (i > 0) {
		y1 = (unsigned char)src[i--];
		v = (unsigned char)src[i--] - 128;
		y0 = (unsigned char)src[i--];
		u = (unsigned char)src[i--] - 128;
		// opencv中存储顺序为bgr
		yuv2rgb(y1, u, v, r, g, b);
		dest[j--] = r;
		dest[j--] = g;
		dest[j--] = b;
		yuv2rgb(y0, u, v, r, g, b);
		dest[j--] = r;
		dest[j--] = g;
		dest[j--] = b;
	}
}

/**
 * @brief IplImage的类型要求为CV_8UC3，并且假定已经对齐
 *
 * @return 是否成功
 */
bool loadImageYUV422FromIplImage(const IplImage *iplImage, ImageYUV422 &image)
{
	unsigned width, height;
	unsigned size;

	width = iplImage->width;
	height = iplImage->height;
	size = width * height;

	if (!size) {
		return false;
	}

	// image为空或者图像大小发生了改变
	if (!image.data || image.width != width || image.height != height) {
		if (image.data) {
			delete image.data;
		}
		image.width = width;
		image.height = height;
		image.data = new unsigned char[size + size];
	}

	rgb2uyvy((unsigned char *)iplImage->imageData, image.data, size);
	
	return true;
}

/**
 * @brief mat的类型要求为CV_8UC3，并且假定已经对齐（AUTO_STEP）
 *
 * @return 是否成功
 */
bool loadImageYUV422FromMat(const cv::Mat &mat, ImageYUV422 &image)
{
	unsigned width, height;
	unsigned size;

	width = mat.cols;
	height = mat.rows;
	size = width * height;

	if (!size) {
		return false;
	}

	// image为空或者图像大小发生了改变
	if (!image.data || image.width != width || image.height != height) {
		if (image.data) {
			delete image.data;
		}
		image.width = width;
		image.height = height;
		image.data = new unsigned char[size + size];
	}

	rgb2uyvy(mat.data, image.data, size);

	return true;
}

bool loadMatFromImageYUV422(const ImageYUV422 &image, cv::Mat &mat)
{
	if (image.height && image.height) {
		mat = cv::Mat(image.height, image.width, CV_8UC3);
		uyvy2rgb(image.data, mat.data, image.height * image.width);
		return true;
	}

	return false;
}

/**
 * @brief 加载配置文件
 */
std::vector<Color> loadConfig(const char *filename)
{
	Color color;
	std::vector<Color> colors;
	FILE *file;
	char buf[256];
	char name[50];
	int ret;

	file = fopen(filename, "r");
	if (!file) {
		std::cerr << "load config fail!" << std::endl;
		return colors;
	}

	while (fgets(buf, sizeof(buf), file)) {
		if (buf[0] == '#') { // 注释
			continue;
		}

		ret = sscanf(buf, "%s (%d, %d, %d) (%d:%d, %d:%d, %d:%d)",
				name, &(color.color_r), &(color.color_g), &(color.color_b),
				&(color.y_min), &(color.y_max),
				&(color.u_min), &(color.u_max),
				&(color.v_min), &(color.v_max));
		if (ret == 10) {
			color.name = name;
			colors.push_back(color);
		} else {
			std::cerr << "config parse error: " << buf << std::endl
				<< "\t" << "ret: " << ret << std::endl;
			return colors;
		}
	}

	return colors;
}

/**
 * @brief 名称为name的颜色对应的序号（从0开始）
 *
 * @return 如果没有对应的颜色，返回-1
 */
int colorIndex(const std::vector<Color> &colors, const char *name)
{
	for (int i = 0; i < colors.size(); ++i) {
		if (colors[i].name == name) {
			return i;
		}
	}
	return -1;
}

}
